clear,clc
%% 参数设置（来自表B4，调整部分参数增强稳定性）
Rr = 0.0013;       % 转子绕组电阻
Xs = 2.9;          % 定子绕组漏抗
Xr = 2.9;          % 转子绕组漏抗
Xm = 2.6;          % 磁感电抗
Tj = 3.4;          % 惯性时间常数
D = 20;            % 增大阻尼系数以抑制振荡（原值5→20）
C = 13.29;         % 中间电容容值
Xl3 = 5;           % 滤波电抗值
omega_s = 1.0;     % 同步角速度

% 计算派生参数
Xn = Xm + Xr;  % 转子侧总阻抗             
Xp = Xm + Xs - (Xm^2)/Xn;  % 定子侧等效阻抗
Ts = 0.01;                 % 减小时间步长（原0.01→0.005）采样时间（降低步长提升精度）
N = 23000;              % 总仿真步数    
n=6;
x = zeros(6, N); 
x(:,1) = [0.8; 0.6; 0.01; 1.0; 0.05; 0.03];  
%% 输入变量定义（限制幅值并平滑变化）
t = (0:N-1)*Ts;
% d轴转子电压（添加小幅波动）
U_dr = 0.2 + 0.02*sin(2*pi*0.2*t);  % 减小输入波动幅度
% q轴转子电压（余弦波动）
U_qr = 0.1 + 0.01*cos(2*pi*0.2*t);% 机械转矩（低频变化）
T_m = 0.8 + 0.05*sin(2*pi*0.1*t);   % 机械转矩平稳变化
I_dl = 0.5 + 0.01*randn(1,N);       % 减小负载噪声% 负载电流（含小噪声）
%% 状态方程离散化（RK4方法，增加U_dc保护）
for k = 1:N-1
    % 直流电压保护（防止过零点）
     x(4,k) = max(x(4,k), 0.1);  
    
    % RK4积分 % 四阶龙格库塔积分
    k1 = state_model(x(:,k), U_dr(k), U_qr(k), T_m(k), I_dl(k), Ts);
    k2 = state_model(x(:,k) + Ts/2*k1, U_dr(k), U_qr(k), T_m(k), I_dl(k), Ts);
    k3 = state_model(x(:,k) + Ts/2*k2, U_dr(k), U_qr(k), T_m(k), I_dl(k), Ts);
    k4 = state_model(x(:,k) + Ts*k3,   U_dr(k), U_qr(k), T_m(k), I_dl(k), Ts);
    x(:,k+1) = x(:,k) + Ts/6*(k1 + 2*k2 + 2*k3 + k4)+ 0.009*randn(6,1);% 添加小量噪声，原始0.001，增加为0.009
end

%% 观测方程（添加量测噪声）（添加高斯白噪声）
m=4;
z = zeros(4, N);
for k = 1:N 
    % 真实量测值 + 量测噪声（SNR≈60dB）
    z(:,k) = meas_model(x(:,k)) + 0.009*randn(4,1);% 添加小量噪声，原始0.001，增加为0.009
    
end
%% 状态方程修正（关键模型调整）
function dx = state_model(x, U_dr, U_qr, T_m, I_dl, Ts)
    Rr = 0.0013;    
    Xm = 2.6;      
    Xn = Xm + 2.9;  
    Tj = 3.4;      
    D = 20;         % 同步修改阻尼系数
    C = 13.29;     
    Xl3 = 5;       
    omega_s = 1.0; 

    E_d = x(1);
    E_q = x(2);
    s = x(3);
    U_dc = max(x(4), 0.1);  % 确保U_dc>0
    I_rd3 = x(5);
    I_rq3 = x(6);
    
    omega_r = omega_s * (1 - s);
    
    % 修正电磁转矩计算（根据论文式(1)）
    Tc = (E_d*I_rd3 + E_q*I_rq3);  % 正确电磁转矩表达式
    
    % 状态方程（修正符号与系数）
    dE_d = omega_s * (-Rr/Xn*E_d + s*E_q + (Rr*Xm/Xn)*I_rq3 - (Xm/Xn)*U_qr);
    dE_q = omega_s * (-Rr/Xn*E_q - s*E_d - (Rr*Xm/Xn)*I_rd3 + (Xm/Xn)*U_dr);

    % 修正转差率方程（增加非线性项补偿）
    ds = (T_m - Tc - D*(s - 0.01) - 0.1*s^3) / Tj; %修正后
    % ds = (T_m - Tc - D*(s - 0.01)) / Tj;  % 原方程，明确阻尼项
    

    % 直流电压方程增加漏电流补偿
    % dU_dc = (I_rd3^2 + I_rq3^2 - I_dl - 0.01*U_dc) / (C * U_dc);
    % 修正直流电压方程（根据论文式(2)）
      dU_dc = (I_rd3^2 + I_rq3^2 - I_dl) / (C * U_dc);%原方程
    
    % 修正网侧变流器方程（根据论文式(3)）
    dI_rd3 = (U_dr - U_dc + Xl3*omega_r*I_rq3) / Xl3;
    dI_rq3 = (U_qr - U_dc - Xl3*omega_r*I_rd3) / Xl3;
    
    dx = [dE_d; dE_q; ds; dU_dc; dI_rd3; dI_rq3];
end

%% 量测方程（保持与论文一致）
function z = meas_model(x)
    Rr = 0.0013;
    Xs = 2.9;
    Xm = 2.6;
    Xn = Xm + 2.9;
    Xp = Xm + Xs - (Xm^2)/Xn;
    omega_s = 1.0;

    E_d = x(1);
    E_q = x(2);
    s = x(3);
    U_dc = x(4);
    
    z = [...
        omega_s * (1 - s); 
        U_dc;
        (Rr*E_d + Xp*E_q)/(Rr^2 + Xp^2) + x(5); 
        (-Xp*E_d + Rr*E_q)/(Rr^2 + Xp^2) + x(6)  
    ];
end
